
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_imu_param.c
  * @author     baiyang
  * @date       2021-10-19
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_imu.h"
#include <parameter/param.h>
#include <common/console/console.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void sensor_imu_acc_offset_set_and_save(uint8_t i, Vector3f_t* offset);
static void sensor_imu_acc_scale_set_and_save(uint8_t i, Vector3f_t* scale);
static void sensor_imu_acc_scale_save(uint8_t i);
static void sensor_imu_acc_id_save(uint8_t i);
static void sensor_imu_acc_id_save2(uint8_t i, int32_t val);
/*----------------------------------variable----------------------------------*/
static param_t var_info[] = {
    PARAM_DEFINE_FLOAT(INS_GYROFFS_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_GYROFFS_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_GYROFFS_Z, 0.0f),
    PARAM_DEFINE_FLOAT(INS_GYR2OFFS_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_GYR2OFFS_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_GYR2OFFS_Z, 0.0f),
    PARAM_DEFINE_FLOAT(INS_GYR3OFFS_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_GYR3OFFS_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_GYR3OFFS_Z, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACCSCAL_X, 1.0f),
    PARAM_DEFINE_FLOAT(INS_ACCSCAL_Y, 1.0f),
    PARAM_DEFINE_FLOAT(INS_ACCSCAL_Z, 1.0f),
    PARAM_DEFINE_FLOAT(INS_ACCOFFS_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACCOFFS_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACCOFFS_Z, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACC2SCAL_X, 1.0f),
    PARAM_DEFINE_FLOAT(INS_ACC2SCAL_Y, 1.0f),
    PARAM_DEFINE_FLOAT(INS_ACC2SCAL_Z, 1.0f),
    PARAM_DEFINE_FLOAT(INS_ACC2OFFS_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACC2OFFS_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACC2OFFS_Z, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACC3SCAL_X, 1.0f),
    PARAM_DEFINE_FLOAT(INS_ACC3SCAL_Y, 1.0f),
    PARAM_DEFINE_FLOAT(INS_ACC3SCAL_Z, 1.0f),
    PARAM_DEFINE_FLOAT(INS_ACC3OFFS_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACC3OFFS_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_ACC3OFFS_Z, 0.0f),
    PARAM_DEFINE_INT16(INS_GYRO_FILTER, 20),
    PARAM_DEFINE_INT16(INS_ACCEL_FILTER, 20),
    PARAM_DEFINE_INT8(INS_USE, 1),
    PARAM_DEFINE_INT8(INS_USE2, 1),
    PARAM_DEFINE_INT8(INS_USE3, 1),
    PARAM_DEFINE_FLOAT(INS_STILL_THRESH, 2.5f),
    PARAM_DEFINE_INT8(INS_GYR_CAL, 1),
    PARAM_DEFINE_INT8(INS_TRIM_OPTION, 1),
    PARAM_DEFINE_INT8(INS_ACC_BODYFIX, 2),
    PARAM_DEFINE_FLOAT(INS_POS1_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_POS1_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_POS1_Z, 0.0f),
    PARAM_DEFINE_FLOAT(INS_POS2_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_POS2_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_POS2_Z, 0.0f),
    PARAM_DEFINE_FLOAT(INS_POS3_X, 0.0f),
    PARAM_DEFINE_FLOAT(INS_POS3_Y, 0.0f),
    PARAM_DEFINE_FLOAT(INS_POS3_Z, 0.0f),
    PARAM_DEFINE_INT32(INS_GYR_ID, 0),
    PARAM_DEFINE_INT32(INS_GYR2_ID, 0),
    PARAM_DEFINE_INT32(INS_GYR3_ID, 0),
    PARAM_DEFINE_INT32(INS_ACC_ID, 0),
    PARAM_DEFINE_INT32(INS_ACC2_ID, 0),
    PARAM_DEFINE_INT32(INS_ACC3_ID, 0),
    PARAM_DEFINE_INT8(INS_FAST_SAMPLE, 1),
    PARAM_DEFINE_INT8(INS_ENABLE_MASK, 0x7F),
    PARAM_DEFINE_INT8(INS_GYRO_RATE, 0),
};
PARAM_GROUP_DEFINE(INS, var_info);

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void sensor_imu_assign_param()
{
    sensor_imu * _imu = sensor_imu_get_singleton();

    param_link_variable(PARAM_ID(INS, INS_GYROFFS_X), &_imu->_gyro_offset[0].x);
    param_link_variable(PARAM_ID(INS, INS_GYROFFS_Y), &_imu->_gyro_offset[0].y);
    param_link_variable(PARAM_ID(INS, INS_GYROFFS_Z), &_imu->_gyro_offset[0].z);

    param_link_variable(PARAM_ID(INS, INS_GYR2OFFS_X), &_imu->_gyro_offset[1].x);
    param_link_variable(PARAM_ID(INS, INS_GYR2OFFS_Y), &_imu->_gyro_offset[1].y);
    param_link_variable(PARAM_ID(INS, INS_GYR2OFFS_Z), &_imu->_gyro_offset[1].z);

    param_link_variable(PARAM_ID(INS, INS_GYR3OFFS_X), &_imu->_gyro_offset[2].x);
    param_link_variable(PARAM_ID(INS, INS_GYR3OFFS_Y), &_imu->_gyro_offset[2].y);
    param_link_variable(PARAM_ID(INS, INS_GYR3OFFS_Z), &_imu->_gyro_offset[2].z);

    param_link_variable(PARAM_ID(INS, INS_ACCSCAL_X), &_imu->_accel_scale[0].x);
    param_link_variable(PARAM_ID(INS, INS_ACCSCAL_Y), &_imu->_accel_scale[0].y);
    param_link_variable(PARAM_ID(INS, INS_ACCSCAL_Z), &_imu->_accel_scale[0].z);
    param_link_variable(PARAM_ID(INS, INS_ACCOFFS_X), &_imu->_accel_offset[0].x);
    param_link_variable(PARAM_ID(INS, INS_ACCOFFS_Y), &_imu->_accel_offset[0].y);
    param_link_variable(PARAM_ID(INS, INS_ACCOFFS_Z), &_imu->_accel_offset[0].z);

    param_link_variable(PARAM_ID(INS, INS_ACC2SCAL_X), &_imu->_accel_scale[1].x);
    param_link_variable(PARAM_ID(INS, INS_ACC2SCAL_Y), &_imu->_accel_scale[1].y);
    param_link_variable(PARAM_ID(INS, INS_ACC2SCAL_Z), &_imu->_accel_scale[1].z);
    param_link_variable(PARAM_ID(INS, INS_ACC2OFFS_X), &_imu->_accel_offset[1].x);
    param_link_variable(PARAM_ID(INS, INS_ACC2OFFS_Y), &_imu->_accel_offset[1].y);
    param_link_variable(PARAM_ID(INS, INS_ACC2OFFS_Z), &_imu->_accel_offset[1].z);

    param_link_variable(PARAM_ID(INS, INS_ACC3SCAL_X), &_imu->_accel_scale[2].x);
    param_link_variable(PARAM_ID(INS, INS_ACC3SCAL_Y), &_imu->_accel_scale[2].y);
    param_link_variable(PARAM_ID(INS, INS_ACC3SCAL_Z), &_imu->_accel_scale[2].z);
    param_link_variable(PARAM_ID(INS, INS_ACC3OFFS_X), &_imu->_accel_offset[2].x);
    param_link_variable(PARAM_ID(INS, INS_ACC3OFFS_Y), &_imu->_accel_offset[2].y);
    param_link_variable(PARAM_ID(INS, INS_ACC3OFFS_Z), &_imu->_accel_offset[2].z);

    param_link_variable(PARAM_ID(INS, INS_GYRO_FILTER), &_imu->_gyro_filter_cutoff);
    param_link_variable(PARAM_ID(INS, INS_ACCEL_FILTER), &_imu->_accel_filter_cutoff);

    param_link_variable(PARAM_ID(INS, INS_USE), &_imu->_use[0]);
    param_link_variable(PARAM_ID(INS, INS_USE2), &_imu->_use[1]);
    param_link_variable(PARAM_ID(INS, INS_USE3), &_imu->_use[2]);

    param_link_variable(PARAM_ID(INS, INS_STILL_THRESH), &_imu->_still_threshold);
    param_link_variable(PARAM_ID(INS, INS_GYR_CAL), &_imu->_gyro_cal_timing);

    param_link_variable(PARAM_ID(INS, INS_TRIM_OPTION), &_imu->_trim_option);
    param_link_variable(PARAM_ID(INS, INS_ACC_BODYFIX), &_imu->_acc_body_aligned);

    param_link_variable(PARAM_ID(INS, INS_POS1_X), &_imu->_accel_pos[0].x);
    param_link_variable(PARAM_ID(INS, INS_POS1_Y), &_imu->_accel_pos[0].y);
    param_link_variable(PARAM_ID(INS, INS_POS1_Z), &_imu->_accel_pos[0].z);

    param_link_variable(PARAM_ID(INS, INS_POS2_X), &_imu->_accel_pos[1].x);
    param_link_variable(PARAM_ID(INS, INS_POS2_Y), &_imu->_accel_pos[1].y);
    param_link_variable(PARAM_ID(INS, INS_POS2_Z), &_imu->_accel_pos[1].z);

    param_link_variable(PARAM_ID(INS, INS_POS3_X), &_imu->_accel_pos[2].x);
    param_link_variable(PARAM_ID(INS, INS_POS3_Y), &_imu->_accel_pos[2].y);
    param_link_variable(PARAM_ID(INS, INS_POS3_Z), &_imu->_accel_pos[2].z);

    param_link_variable(PARAM_ID(INS, INS_GYR_ID), &_imu->_gyro_id[0]);
    param_link_variable(PARAM_ID(INS, INS_GYR2_ID), &_imu->_gyro_id[1]);
    param_link_variable(PARAM_ID(INS, INS_GYR3_ID), &_imu->_gyro_id[2]);

    param_link_variable(PARAM_ID(INS, INS_ACC_ID), &_imu->_accel_id[0]);
    param_link_variable(PARAM_ID(INS, INS_ACC2_ID), &_imu->_accel_id[1]);
    param_link_variable(PARAM_ID(INS, INS_ACC3_ID), &_imu->_accel_id[2]);

    param_link_variable(PARAM_ID(INS, INS_FAST_SAMPLE), &_imu->_fast_sampling_mask);
    param_link_variable(PARAM_ID(INS, INS_ENABLE_MASK), &_imu->_enable_mask);
    param_link_variable(PARAM_ID(INS, INS_GYRO_RATE), &_imu->_fast_sampling_rate);
}

void sensor_imu_acal_event_failure()
{
    sensor_imu * _imu = sensor_imu_get_singleton();

    for (uint8_t i=0; i<_imu->_accel_count; i++) {
        if (i == 0) {
            param_set_obj_and_notify(PARAM_ID(INS, INS_ACCSCAL_X), 1.0);
            param_set_obj_and_notify(PARAM_ID(INS, INS_ACCSCAL_Y), 1.0);
            param_set_obj_and_notify(PARAM_ID(INS, INS_ACCSCAL_Z), 1.0);
            param_set_obj_and_notify(PARAM_ID(INS, INS_ACCOFFS_X), 0.0);
            param_set_obj_and_notify(PARAM_ID(INS, INS_ACCOFFS_Y), 0.0);
            param_set_obj_and_notify(PARAM_ID(INS, INS_ACCOFFS_Z), 0.0);
        } else if (i == 1) {
            param_set_obj_and_notify(PARAM_ID(INS, INS_ACC2SCAL_X), 1.0);
            param_set_obj_and_notify(PARAM_ID(INS, INS_ACC2SCAL_Y), 1.0);
            param_set_obj_and_notify(PARAM_ID(INS, INS_ACC2SCAL_Z), 1.0);
            param_set_obj_and_notify(PARAM_ID(INS, INS_ACC2OFFS_X), 0.0);
            param_set_obj_and_notify(PARAM_ID(INS, INS_ACC2OFFS_Y), 0.0);
            param_set_obj_and_notify(PARAM_ID(INS, INS_ACC2OFFS_Z), 0.0);
        } else if (i == 2) {
            param_set_obj_and_notify(PARAM_ID(INS, INS_ACC3SCAL_X), 1.0);
            param_set_obj_and_notify(PARAM_ID(INS, INS_ACC3SCAL_Y), 1.0);
            param_set_obj_and_notify(PARAM_ID(INS, INS_ACC3SCAL_Z), 1.0);
            param_set_obj_and_notify(PARAM_ID(INS, INS_ACC3OFFS_X), 0.0);
            param_set_obj_and_notify(PARAM_ID(INS, INS_ACC3OFFS_Y), 0.0);
            param_set_obj_and_notify(PARAM_ID(INS, INS_ACC3OFFS_Z), 0.0);
        }
    }
}

/*
    set and save accelerometer bias along with trim calculation
*/
void sensor_imu_acal_save_calibrations()
{
    sensor_imu * _imu = sensor_imu_get_singleton();

    Vector3f_t bias = {0.0f, 0.0f, 0.0f};
    Vector3f_t gain = {0.0f, 0.0f, 0.0f};
    for (uint8_t i=0; i<_imu->_accel_count; i++) {
        if (accelcal_get_status(&_imu->_accel_calibrator[i]) == ACCEL_CAL_SUCCESS) {
            accelcal_get_calibration2(&_imu->_accel_calibrator[i], &bias, &gain);
            sensor_imu_acc_offset_set_and_save(i, &bias);
            sensor_imu_acc_scale_set_and_save(i, &gain);
            sensor_imu_acc_id_save(i);
            _imu->_accel_id_ok[i] = true;
#if HAL_INS_TEMPERATURE_CAL_ENABLE
            caltemp_accel[i].set_and_save(get_temperature(i));
#endif
        } else {
            sensor_imu_acc_offset_set_and_save(i, &bias);
            sensor_imu_acc_scale_set_and_save(i, &gain);
#if HAL_INS_TEMPERATURE_CAL_ENABLE
            caltemp_accel[i].set_and_save(-300);
#endif
        }
    }

    Vector3f_t bias_tmp = {0.0f, 0.0f, 0.0f};
    Vector3f_t gain_tmp = {0.0f, 0.0f, 0.0f};
    // clear any unused accels
    for (uint8_t i=_imu->_accel_count; i<INS_MAX_INSTANCES; i++) {
        sensor_imu_acc_id_save2(i, 0);
        sensor_imu_acc_offset_set_and_save(i, &bias_tmp);
        sensor_imu_acc_scale_set_and_save(i, &gain_tmp);
#if HAL_INS_TEMPERATURE_CAL_ENABLE
        caltemp_accel[i].set_and_save_ifchanged(-300);
#endif
    }
    
    Vector3f_t aligned_sample;
    Vector3f_t misaligned_sample;
    switch(_imu->_trim_option) {
        case 0:
            break;
        case 1:
            // The first level step of accel cal will be taken as gnd truth,
            // i.e. trim will be set as per the output of primary accel from the level step
            sensor_imu_get_primary_accel_cal_sample_avg(0, &aligned_sample);
            vec3_zero(&_imu->_trim_rad);
            sensor_imu_calculate_trim(&aligned_sample, &_imu->_trim_rad);
            _imu->_new_trim = true;
            break;
        case 2:
            // Reference accel is truth, in this scenario there is a reference accel
            // as mentioned in ACC_BODY_ALIGNED
            if (sensor_imu_get_primary_accel_cal_sample_avg(0, &misaligned_sample) && sensor_imu_get_fixed_mount_accel_cal_sample(0,&aligned_sample)) {
                // determine trim from aligned sample vs misaligned sample
                Vector3f_t cross;
                vec3_cross(&cross, &misaligned_sample, &aligned_sample);

                float dot = vec3_dot(&misaligned_sample, &aligned_sample);
                Quat_t q = {math_sqrtf(sq(vec3_length(&misaligned_sample))*sq(vec3_length(&aligned_sample)))+dot, cross.x, cross.y, cross.z};
                quat_norm(&q);
                _imu->_trim_rad.x = quat_get_roll(&q);
                _imu->_trim_rad.y = quat_get_pitch(&q);
                _imu->_trim_rad.z = 0;
                _imu->_new_trim = true;
            }
            break;
        default:
            _imu->_new_trim = false;
            /* no break */
    }

    if (fabsf(_imu->_trim_rad.x) > radians(HAL_INS_TRIM_LIMIT_DEG) ||
        fabsf(_imu->_trim_rad.y) > radians(HAL_INS_TRIM_LIMIT_DEG) ||
        fabsf(_imu->_trim_rad.z) > radians(HAL_INS_TRIM_LIMIT_DEG)) {
        console_printf("ERR: Trim over maximum of %.1f degrees!!", (float)(HAL_INS_TRIM_LIMIT_DEG));
        _imu->_new_trim = false;  //we have either got faulty level during acal or highly misaligned accelerometers
    }

    _imu->_accel_cal_requires_reboot = true;
}

// force save of current calibration as valid
void sensor_imu_force_save_calibration(void)
{
    sensor_imu * _imu = sensor_imu_get_singleton();

    for (uint8_t i=0; i<_imu->_accel_count; i++) {
        if (_imu->_accel_id[i] != 0) {
            sensor_imu_acc_id_save(i);
            // we also save the scale as the default of 1.0 may be
            // over a stored value of 0.0
            sensor_imu_acc_scale_save(i);
            _imu->_accel_id_ok[i] = true;
        }
    }
}

void sensor_imu_gyr_scale_set_and_save(uint8_t i, Vector3f_t* scale)
{
    if (i == 0) {
        param_set_and_save(PARAM_ID(INS, INS_GYROFFS_X), scale->x);
        param_set_and_save(PARAM_ID(INS, INS_GYROFFS_Y), scale->y);
        param_set_and_save(PARAM_ID(INS, INS_GYROFFS_Z), scale->z);
    } else if (i == 1) {
        param_set_and_save(PARAM_ID(INS, INS_GYR2OFFS_X), scale->x);
        param_set_and_save(PARAM_ID(INS, INS_GYR2OFFS_Y), scale->y);
        param_set_and_save(PARAM_ID(INS, INS_GYR2OFFS_Z), scale->z);
    } else if (i == 2) {
        param_set_and_save(PARAM_ID(INS, INS_GYR3OFFS_X), scale->x);
        param_set_and_save(PARAM_ID(INS, INS_GYR3OFFS_Y), scale->y);
        param_set_and_save(PARAM_ID(INS, INS_GYR3OFFS_Z), scale->z);
    }
}

void sensor_imu_gyr_scale_save(uint8_t i)
{
    if (i == 0) {
        param_save_obj(PARAM_ID(INS, INS_GYROFFS_X));
        param_save_obj(PARAM_ID(INS, INS_GYROFFS_Y));
        param_save_obj(PARAM_ID(INS, INS_GYROFFS_Z));
    } else if (i == 1) {
        param_save_obj(PARAM_ID(INS, INS_GYR2OFFS_X));
        param_save_obj(PARAM_ID(INS, INS_GYR2OFFS_Y));
        param_save_obj(PARAM_ID(INS, INS_GYR2OFFS_Z));
    } else if (i == 2) {
        param_save_obj(PARAM_ID(INS, INS_GYR3OFFS_X));
        param_save_obj(PARAM_ID(INS, INS_GYR3OFFS_Y));
        param_save_obj(PARAM_ID(INS, INS_GYR3OFFS_Z));
    }
}

void sensor_imu_gyr_id_set_and_save(uint8_t i, int32_t id)
{
    if (i == 0) {
        param_set_and_save(PARAM_ID(INS, INS_GYR_ID), id);
    } else if (i == 1) {
        param_set_and_save(PARAM_ID(INS, INS_GYR2_ID), id);
    } else if (i == 2) {
        param_set_and_save(PARAM_ID(INS, INS_GYR3_ID), id);
    }
}

void sensor_imu_gyr_id_save(uint8_t i)
{
    if (i == 0) {
        param_save_obj(PARAM_ID(INS, INS_GYR_ID));
    } else if (i == 1) {
        param_save_obj(PARAM_ID(INS, INS_GYR2_ID));
    } else if (i == 2) {
        param_save_obj(PARAM_ID(INS, INS_GYR3_ID));
    }
}

void sensor_imu_fast_sampling_mask_set(int8_t val)
{
    param_set_obj(PARAM_ID(INS, INS_FAST_SAMPLE), val);
}

static void sensor_imu_acc_offset_set_and_save(uint8_t i, Vector3f_t* offset)
{
    if (i == 0) {
        param_set_and_save(PARAM_ID(INS, INS_ACCOFFS_X), offset->x);
        param_set_and_save(PARAM_ID(INS, INS_ACCOFFS_Y), offset->y);
        param_set_and_save(PARAM_ID(INS, INS_ACCOFFS_Z), offset->z);
    } else if (i == 1) {
        param_set_and_save(PARAM_ID(INS, INS_ACC2OFFS_X), offset->x);
        param_set_and_save(PARAM_ID(INS, INS_ACC2OFFS_Y), offset->y);
        param_set_and_save(PARAM_ID(INS, INS_ACC2OFFS_Z), offset->z);
    } else if (i == 2) {
        param_set_and_save(PARAM_ID(INS, INS_ACC3OFFS_X), offset->x);
        param_set_and_save(PARAM_ID(INS, INS_ACC3OFFS_Y), offset->y);
        param_set_and_save(PARAM_ID(INS, INS_ACC3OFFS_Z), offset->z);
    }
}

static void sensor_imu_acc_scale_set_and_save(uint8_t i, Vector3f_t* scale)
{
    if (i == 0) {
        param_set_and_save(PARAM_ID(INS, INS_ACCSCAL_X), scale->x);
        param_set_and_save(PARAM_ID(INS, INS_ACCSCAL_Y), scale->y);
        param_set_and_save(PARAM_ID(INS, INS_ACCSCAL_Z), scale->z);
    } else if (i == 1) {
        param_set_and_save(PARAM_ID(INS, INS_ACC2SCAL_X), scale->x);
        param_set_and_save(PARAM_ID(INS, INS_ACC2SCAL_Y), scale->y);
        param_set_and_save(PARAM_ID(INS, INS_ACC2SCAL_Z), scale->z);
    } else if (i == 2) {
        param_set_and_save(PARAM_ID(INS, INS_ACC3SCAL_X), scale->x);
        param_set_and_save(PARAM_ID(INS, INS_ACC3SCAL_Y), scale->y);
        param_set_and_save(PARAM_ID(INS, INS_ACC3SCAL_Z), scale->z);
    }
}

static void sensor_imu_acc_scale_save(uint8_t i)
{
    if (i == 0) {
        param_save_obj(PARAM_ID(INS, INS_ACCSCAL_X));
        param_save_obj(PARAM_ID(INS, INS_ACCSCAL_Y));
        param_save_obj(PARAM_ID(INS, INS_ACCSCAL_Z));
    } else if (i == 1) {
        param_save_obj(PARAM_ID(INS, INS_ACC2SCAL_X));
        param_save_obj(PARAM_ID(INS, INS_ACC2SCAL_Y));
        param_save_obj(PARAM_ID(INS, INS_ACC2SCAL_Z));
    } else if (i == 2) {
        param_save_obj(PARAM_ID(INS, INS_ACC3SCAL_X));
        param_save_obj(PARAM_ID(INS, INS_ACC3SCAL_Y));
        param_save_obj(PARAM_ID(INS, INS_ACC3SCAL_Z));
    }
}

static void sensor_imu_acc_id_save(uint8_t i)
{
    if (i == 0) {
        param_save_obj(PARAM_ID(INS, INS_ACC_ID));
    } else if (i == 1) {
        param_save_obj(PARAM_ID(INS, INS_ACC2_ID));
    } else if (i == 2) {
        param_save_obj(PARAM_ID(INS, INS_ACC3_ID));
    }
}

static void sensor_imu_acc_id_save2(uint8_t i, int32_t val)
{
    if (i == 0) {
        param_set_and_save(PARAM_ID(INS, INS_ACC_ID), val);
    } else if (i == 1) {
        param_set_and_save(PARAM_ID(INS, INS_ACC2_ID), val);
    } else if (i == 2) {
        param_set_and_save(PARAM_ID(INS, INS_ACC3_ID), val);
    }
}

/*------------------------------------test------------------------------------*/


